﻿#include "Collision.h"


Collision::Collision(void)
{

}

Collision::~Collision(void)
{

}

//bool Collision::isCollided(CPolygon A, CPolygon B, Vector &MTD)
//{
//	int n_a = A.getLength();	//số vector của đa giác A
//	int n_b = B.getLength();	//số vector của đa giác B
//
//	Vector N,E,E1,E2;
//	Vector* PushVectorList;
//
//	PushVectorList = new Vector[n_a + n_b];
//
//	for (int i=0,j=n_a - 1;i<n_a;j=i,i++)
//	{
//		E1 = A.getVertice(i);
//		E2 = A.getVertice(j);
//		E = E1 - E2;
//		N = Vector(- E.getY(), E.getX());
//		if (this->axisSeperatePolygon(N,A,B,PushVectorList[i]))
//			return false;
//	}
//
//	for (int i=0,j=n_b - 1;i<n_b;j=i,i++)
//	{
//		E = B.getVertice(i) - B.getVertice(j);
//		N = Vector(- E.getY(), E.getX());
//		if (this->axisSeperatePolygon(N,A,B,PushVectorList[n_a + i]))
//			return false;
//	}
//
//	//tìm pushvector có độ lớn nhỏ nhất
//	MTD = findMTD(PushVectorList,n_a + n_b);
//
//	Vector D = A.getVertice(0) - B.getVertice(0);
//
//	if (D * MTD < 0)
//		MTD = -MTD;
//
//	return true;
//}

bool Collision::isCollided(CPolygon A, CPolygon B, Vector &MTD, Vector Vel, Vector &PushVelVector, bool &willCollide)
{
	int n_a = A.getLength();	//số vector của đa giác A
	int n_b = B.getLength();	//số vector của đa giác B

	int tem;

	willCollide = false;

	Vector N,E,E1,E2;
	Vector* PushVectorList;
	Vector* PushVelVectorList;

	PushVectorList = new Vector[n_a + n_b];
	PushVelVectorList = new Vector[n_a + n_b];
	int numPushVelVector = 0;

	for (int i=0,j=n_a - 1;i<n_a;j=i,i++)
	{
		E1 = A.getVertice(i);
		E2 = A.getVertice(j);
		E = E1 - E2;
		N = Vector(- E.getY(), E.getX());
		tem = this->axisSeperatePolygon(N,A,B,PushVectorList[i],Vel,PushVelVectorList,numPushVelVector);
		if (tem == -1)
			return false;
		else if (tem == 0)
			willCollide = true;
	}

	for (int i=0,j=n_b - 1;i<n_b;j=i,i++)
	{
		E = B.getVertice(i) - B.getVertice(j);
		N = Vector(- E.getY(), E.getX());
		tem = this->axisSeperatePolygon(N,A,B,PushVectorList[n_a + i],Vel,PushVelVectorList,numPushVelVector);
		if (tem == -1)
			return false;
		else if (tem == 0)
			willCollide = true;
		//if (tem != 1) return false;
	}

	Vector D = A.getVertice(0) - B.getVertice(0);
	
	//va chạm, tìm pushvector có độ lớn nhỏ nhất
	if (!willCollide)
	{
		MTD = findMTD(PushVectorList,n_a + n_b);
	//tìm pushvelvector có độ lớn lớn nhất
		if (D * MTD < 0)
			MTD = -MTD;
	}
	else
	{
		PushVelVector = findPushVelVector(PushVelVectorList,numPushVelVector);
		if (PushVelVector * Vel < 0)
			PushVelVector = -PushVelVector;
		//MessageBox(NULL,L"Haha",L"Hoho",MB_OK);
	}

	delete[] PushVectorList;
	delete[] PushVelVectorList;

	return true;
}

//bool Collision::isCollided(MovingObject* OA, MovingObject* OB)
//{
//	RECT arect = OA->getRect();
//	RECT brect = OB->getRect();
//	CPolygon A = OA->getPolygon();
//	CPolygon B = OB->getPolygon();
//
//	Vector Vel = OA->getVelocityVector();
//
//	Vector MTD,PushVelVector;
//	bool willCollide;
//	bool result = isCollided(A,B,MTD,OA->getVelocityVector(),PushVelVector,willCollide);
//
//	//xảy ra va chạm
//	if (result == true)
//	{
//		//sẽ va chạm trong khoảng từ vị trí hiện tại đến vị trí trong frame tiếp theo
//		if (willCollide)
//		{
//			if (PushVelVector * Vel > 0)
//				PushVelVector = -PushVelVector;
//			OA->setPushBackVector(PushVelVector);
//		}
//		else
//		{
//			OA->Translate((MTD));
//			//OA->Translate((float)1/2 * (MTD));
//			////OA->setVelocity(OA->getVelocityVector() + (float)1/2 * (MTD));
//			Vector vel = OA->getVelocityVector() + (float)1/2*OB->getVelocityVector();
//			Vector normalVector = (MTD.Normalise());
//			float f = (vel*normalVector);
//			Vector afterCollideVector = vel - ((1 + 1)*(f*normalVector));
//			OA->setVelocity(afterCollideVector);
//			//OB->Translate((float)1/2 * (-MTD));
//		}
//	}
//
//	return result;
//}

bool Collision::isCollided(MovingObject* OA, GObject* OB)
{
	RECT arect = OA->getRect();
	RECT brect = OB->getRect();
	CPolygon A = CPolygon(arect);
	CPolygon B = CPolygon(brect);

	Vector Vel = OA->getVelocityVector();

	Vector MTD,PushVelVector;
	bool willCollide;
	bool result = isCollided(A,B,MTD,OA->getVelocityVector(),PushVelVector,willCollide);

	//Vector v=Vector();

	//xảy ra va chạm
	if (result == true)
	{
		//sẽ va chạm trong khoảng từ vị trí hiện tại đến vị trí trong frame tiếp theo
		if (willCollide)
		{
			if (PushVelVector * Vel > 0)
				PushVelVector = -PushVelVector;
			OA->setPushBackVector(PushVelVector);
		}
		else
		{
			OA->Translate((MTD));
			//OA->Translate((float)1/2 * (MTD));
			////OA->setVelocity(OA->getVelocityVector() + (float)1/2 * (MTD));


			Vector vel = OA->getVelocityVector();// + (float)1/2*OB->getVelocityVector();
			Vector normalVector = (MTD.Normalise());
			float f = (vel*normalVector);
			Vector afterCollideVector = vel - ((1.f)*(f*normalVector));
			OA->setVelocity(afterCollideVector);

			//v = MTD;
			//OB->Translate((float)1/2 * (-MTD));
		}
	}

	return result;
}

void Collision::getInterval(CPolygon polygon, Vector Axis, float &min, float &max)
{
	float d = polygon.getVertice(0) * Axis; // tích vô hướng của 2 vector: Vector có đỉnh 0 và vector Axis
	min = max = d;
	int numVertices = polygon.getLength();
	for (int i=1;i<numVertices;i++)
	{
		d = polygon.getVertice(i) * Axis;
		if (d < min) min = d;
		else if (d > max) max = d;
	}
}

int Collision::axisSeperatePolygon(Vector Axis,CPolygon A, CPolygon B, Vector &PushVector,Vector Vel, Vector* PushVelVectorList, int &numPushVelVector)
{
	float min1,min2,max1,max2;
	PushVector = Axis;

	getInterval(A,Axis,min1,max1);
	getInterval(B,Axis,min2,max2);

	float d1 = max1 - min2;
	float d2 = max2 - min1;

	//không xảy ra va chạm
	if (d2 < 0 || d1 < 0)
	{
		d2 = fabs(d2);
		d1 = fabs(d1);
		float s = (d1 < d2) ? d1:d2;
		s = (float)s/sqrt(Axis.Length());
		float v = fabs(Vel * Axis);
		if (s >= v)
			return -1;
		else
		{
			numPushVelVector++;
			float k = (v-s) / (Axis*Axis);
			PushVelVectorList[numPushVelVector - 1] = k * Axis;
			return 0; //willCollide
		}
	}

	else
	{
		float depth = d1 < d2 ? d1:d2;

		float squareAxisLength = Axis * Axis;

		PushVector = Axis * (float)(depth/squareAxisLength);
		return 1; //collided
	}
}

Vector Collision::findMTD(Vector* PushVectorList,int numItem)
{
	float minMagnitude = PushVectorList[0] * PushVectorList[0];
	int i=0;
	Vector vt;
	float curMagnitude;
	int index = 0;
	for (int i=0;i<numItem;i++)
	{
		curMagnitude = PushVectorList[i] * PushVectorList[i];
		if (curMagnitude < minMagnitude)
		{
			minMagnitude = curMagnitude;
			index = i;
		}
	}

	return PushVectorList[index];
}

Vector Collision::findPushVelVector(Vector *PushVelVectorList, int numItem)
{
	/*float maxMagnitude = PushVelVectorList[0] * PushVelVectorList[0];
	float curMagnitude;
	int index = 0;
	for (int i=1;i<numItem;i++)
	{
		curMagnitude = PushVelVectorList[i] * PushVelVectorList[i];
		if (curMagnitude > maxMagnitude)
		{
			maxMagnitude = curMagnitude;
			index = i;
		}
	}
	
	return PushVelVectorList[index];*/
	return Vector();

}